/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/*
*********************************************************************************************************
* 函数名: DMA_Usart_Send
* 功能说明: 串口发送功能函数
* 输入: buf，len
* 输出:无
*********************************************************************************************************
*/
void DMA_Usart_Send(uint8_t *buf,uint8_t len)
{
 if(HAL_UART_Transmit_DMA(&huart1, buf,len)!= HAL_OK)
  {
   Error_Handler();
  }

}

/*
*********************************************************************************************************
* 函数名: DMA_Usart1_Read
* 功能说明: 串口接收功能函数
* 输入: Data,len
* 输出: 无
*********************************************************************************************************
*/
void DMA_Usart1_Read(uint8_t *Data,uint8_t len)//串口接收封装
{
	HAL_UART_Receive_DMA(&huart1,Data,len);//重新打开DMA接收
}

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
int v,status,r;//通信数据 速度 转向状态  转弯半径
int LEFT=0,RIGHT=0;//编码器读数
int L,R;//两路设定速度
int B=200;//小车宽度
int VL;//编码器转换速度
int VR;//编码器转换速度
int pwm1=0;
int pwm2=0;
//PID运算相关
float Kp = 3, Ki = 0.01, Kd = 0;//定义比例、积分、微分
int SetSpeedL;//履带设定速度
int ActualSpeedL;//实际输出变量，即采样回来的输出变
int ErrL; //误差
int Err1L;//上一次误差
int Err2L;//前一次误差
int Power_outL;
int SetSpeedR;//履带设定速度
int ActualSpeedR;//实际输出变量，即采样回来的输出变
int ErrR; //误差
int Err1R;//上一次误差
int Err2R;//前一次误差
int Power_outR;

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/*
*********************************************************************************************************
* 函数名: control_1
* 特别说明：电机输入pwm波为0时最快，为了方便控制，进行一些转化，最快时pwm=3600 最慢时为0，为负数则反转
* 功能说明: 控制左边电机
* 输入: pwm
* 输出: 无
*********************************************************************************************************
*/
void control_1(int pwmL)
{
if(pwmL>0)
	{
	HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_SET);//使能EN1=1
	HAL_GPIO_WritePin(GPIOA, DIR1_Pin, GPIO_PIN_SET);//方向DIR1=1
	TIM1->CCR1 =3600-pwmL;
	}
else if(pwmL==0)
 {
	 HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_RESET);//使能EN1=0
 }
else if(pwmL<0)
 	{
 	HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_SET);//使能EN1=1
 	HAL_GPIO_WritePin(GPIOA, DIR1_Pin, GPIO_PIN_RESET);//方向DIR1=0
 	TIM1->CCR1 =3600+pwmL;
    }
else if(pwmL<-3600)
	{
	pwmL=-3600;
	HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_SET);//使能EN1=1
	HAL_GPIO_WritePin(GPIOA, DIR1_Pin, GPIO_PIN_RESET);//方向DIR1=0
	TIM1->CCR1 =3600+pwmL;
	}
else if(pwmL>3600)
	{
	pwmL=3600;
	HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_SET);//使能EN1=1
	HAL_GPIO_WritePin(GPIOA, DIR1_Pin, GPIO_PIN_SET);//方向DIR1=1
	TIM1->CCR1 =3600-pwmL;
	}
}
 /*
 *********************************************************************************************************
 * 函数名:control_2
 * 功能说明: 控制电机2
 * 输入: pwm
 * 输出: 无
 *********************************************************************************************************
 */
 void control_2(int pwmR) //控制右边电机
 {
 if(pwmR>0)
  {
 	HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_SET);//使能EN2=1
 	HAL_GPIO_WritePin(GPIOA, DIR2_Pin, GPIO_PIN_SET);//方向DIR2=1
 	TIM4->CCR4 =3600-pwmR;
  }
 else if(pwmR==0)
  {
 	 HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_RESET);//使能EN2=0
  }
 else if(pwmR<0)
  {
  	HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_SET);//使能EN2=1
  	HAL_GPIO_WritePin(GPIOA, DIR2_Pin, GPIO_PIN_RESET);//方向DIR2=0
  	TIM4->CCR4 =3600+pwmR;
   }
 else if(pwmR<-3600)
  {
	pwmR=-3600;
	HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_SET);//使能EN2=1
	HAL_GPIO_WritePin(GPIOA, DIR2_Pin, GPIO_PIN_RESET);//方向DIR2=0
	TIM4->CCR4 =3600+pwmR;
  }
 else if(pwmR>3600)
  {
	pwmR=3600;
	HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_SET);//使能EN2=1
	HAL_GPIO_WritePin(GPIOA, DIR2_Pin, GPIO_PIN_SET);//方向DIR2=1
	TIM4->CCR4 =3600-pwmR;
  }
 }
 /*
  *********************************************************************************************************
  * 函数名:PID_Init
  * 功能说明: 初始化pid与相关参数
  * 输入: 无
  * 输出: 无
  *********************************************************************************************************
  */
 void PID_Init()
 {
 	SetSpeedL = 0;
 	ActualSpeedL = 0;
 	ErrL = 0;
 	Err1L = 0;
 	Err2L = 0;
 	Power_outL=0;
 	SetSpeedR = 0;
	ActualSpeedR = 0;
	ErrR = 0;
	Err1R = 0;
	Err2R = 0;
	Power_outR=0;

 }
 /*
 *********************************************************************************************************
 * 函数名: PID_Realize
 * 功能说明: 计算出pwm大小
 * 输入: float actualspeed,float setspeed//实际速度与设定速度
 * 输出: pwm增量
 *********************************************************************************************************
 */
 int PID_RealizeL(int actualspeedL,int setspeedL)//实际速度与设定速度
   {
	 ErrL = setspeedL-actualspeedL;
 	 Power_outL=Power_outL+  Kp*(ErrL-Err1L)+Ki*ErrL+Kd*(ErrL-2*Err1L+Err2L);
 	 if(Power_outL>3600)
		 {
			 Power_outL=3600;
		 }
	else if(Power_outL<-3600)
		 {
 		 Power_outL=-3600;
 	     }
	 Err2L = Err1L;
 	 Err1L = ErrL;
 	 return Power_outL;
 }
 int PID_RealizeR(int actualspeedR,int setspeedR)//实际速度与设定速度
  {
  	 ErrR =setspeedR- actualspeedR ;
  	 Power_outR=Power_outR+  Kp*(ErrR-Err1R)+Ki*ErrR+Kd*(ErrR-2*Err1R+Err2R);
  	 if(Power_outR>3600)
 		 {
 			 Power_outR=3600;
 		 }
 		 else if(Power_outR<-3600)
 		 {
  		 Power_outR=-3600;
  	 }
  	 Err2R = Err1R;
  	 Err1R = ErrR;
  	 return Power_outR;
  }

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
  HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
  __HAL_TIM_SET_COUNTER(&htim2,0x7fff);
  __HAL_TIM_SET_COUNTER(&htim3,0x7fff);
  PID_Init();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
	  HAL_GPIO_WritePin(GPIOA, DIR1_Pin|EN1_Pin|DIR2_Pin|EN2_Pin, GPIO_PIN_RESET);
//获取速度，转向，转弯半径
	  if(recv_end_flag == 1)  //接收完成标志
	  {
		  //数据处理
		   recv_end_flag = 0;//清除接收结束标志
		   v=(rx_buffer[0]-48)*100+(rx_buffer[1]-48)*10+(rx_buffer[2]-48);
		   status=rx_buffer[3]-48;
		   if(status!=0)
		   {
			   r=(rx_buffer[4]-48)*1000+(rx_buffer[5]-48)*100+(rx_buffer[6]-48)*10+(rx_buffer[7]-48);
		   }
		   rx_len = 0;//清除计数
		   for(uint8_t i=0;i<rx_len;i++)
			{
				rx_buffer[i]=0;//清接收缓存
			}
	  }
//将速度,转弯半径,转向状态转化为两路设定速度L R status=0为直行status=1为左转status=2为右转

	  if(status==0)
	  {
		  L=v;
		  R=v;
	  }
	  else if(status==1)
	  {
		  if(r!=0)
		  {
			  L=v-(B*v/(2*r));
			  R=v+(B*v/(2*r));
		  }
		  else if(r==0)
			  {
			  L=-v;
			  R=v;
			  }
	     }
	  else if(status==2)
	  {
		  if(r!=0)
		  {
			  L=v+(B*v/(2*r));
			  R=v-(B*v/(2*r));
		  }
		  else if(r==0)
		  {
			  L=v;
			  R=-v;
		  }
	  }

if((-1000<L)&&(L<1000)&&(-1000<R)&&(R<1000))
{
	//增量PID算法求出两路pwm大小
	pwm1=pwm1+(PID_RealizeL(VL,L));
	pwm2=pwm2+(PID_RealizeR(VR,R));
	printf("L:%d\t R:%d\n\r",L,R);
//pwm驱动电机
	control_1(pwm1);
	control_2(pwm2);
}
else
	printf("ERROR\n\r");
	  //清空数组
//	  for(uint8_t i=0;i<rx_len;i++)
//	  {
//	  		rx_buffer[i]=0;//清接收缓存
//	  }
	  //返回数据写入数组
	//rx_buffer[0]=(uint8_t)VL;
	//rx_buffer[1]=(uint8_t)VR;
	//DMA_Usart_Send(rx_buffer,2);//串口发rx_buffer
//		rx_len = 0;//清除计数
//		for(uint8_t i=0;i<rx_len;i++)
//					{
//						rx_buffer[i]=0;//清接收缓存
//					}

	DMA_Usart1_Read(rx_buffer,BUFFER_SIZE);//重新打开DMA接收
	HAL_Delay(500);//不可删除

	 }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
